using System;
using System.IO;
using eee.Sheffield.PZ.Math;
using System.Drawing.Imaging;
using System.Drawing;

namespace eee.Sheffield.PZ.Imaging
{
	/// <summary>
	/// PZ scanline model, simple model
	/// a pure numerical model
	/// </summary>
	public class PZScanline
	{
		public int _widthOfScanline = 0;		/// width of scanline
		public int _sampleRate = 0;		// sample rate
		public int _length = 0;				// length of data, e.g. fg
										// length = widthofscanline * samplerate;
		
		// scanline data 
		public PZMath_vector _fg = null;				// intensities, sample values
		public PZPoint[] _fp = null;			// sample points coordinate
        public PZMath_vector _ffit = null;			// model-fit result
		
		// model parameters
		public double _c = 0;
		public double _r = 0;
		public double _mu = 0;
		public double _B = 0;

		public string exit_msg = " ";

		public void MemCopyFrom(PZScanline sl)
		{
			_widthOfScanline = sl._widthOfScanline;
			_sampleRate = sl._sampleRate;
			_length = sl._length;
			_c = sl._c;
			_r = sl._r;
			_mu = sl._mu;
			_B = sl._B;

            _fg = new PZMath_vector(sl._fg);
			_fp = new PZPoint [_length];
            Array.Copy(sl._fp, _fp, _length);
            _ffit = new PZMath_vector(sl._ffit);

            //for (int i = 0; i < length; i ++)
            //{
            //    fg[i] = sl.fg[i];
            //    fp[i] = new PZPoint();
            //    fp[i].x = sl.fp[i].x;
            //    fp[i].y = sl.fp[i].y;
            //    ffit[i] = sl.ffit[i];
            //}
		} // MemCopyFrom()

		public bool IsFit()
		{
			bool isfit = true;
			//if (mu >= 1.0)
			//{
			//	isfit = false;
			//	exit_msg += "mu > 1.0 ";
			//}
			if (_c <=2)
			{
				isfit = false;
				exit_msg += "c <= 1 ";
			}
			if (_c >= _widthOfScanline - 2)
			{
				isfit = false;
				exit_msg += "c >= widthofscanline - 2 ";
			}
            if (_r > (double)_widthOfScanline / 4.0 * 3.0)
            {
                isfit = false;
                exit_msg += "r > 3/4 widthofscanline";
            }
			double backpower = 0.0;
			int backcount = 0;
			double signalpower = 0.0;
			int signalcount = 0;
            double signalThreshold = 2.0;

			double lowerbound, upperbound;
			lowerbound = _c - _r < 0 ? 0 : _c - _r;
			upperbound = _c + _r > _length ? _length : _c + _r;
            lowerbound *= _sampleRate;
            upperbound *= _sampleRate;
            
			if (lowerbound < _length
				&& upperbound > 0)
			{
				for (int i = 0; i < lowerbound; i ++)
				{
					backpower += _ffit[i];
					backcount ++;
				}
				for (int i = (int) upperbound; i < _length; i ++)
				{
					backpower += _ffit[i];
					backcount ++;
				}
				for (int i = (int) lowerbound; i < upperbound; i ++)
				{
					signalpower += _ffit[i];
					signalcount ++;
				}
				backpower /= (double) backcount;
				signalpower /= (double) signalcount;

				if (backpower <= signalpower)
				{
					isfit = false;
					exit_msg += "backpower <= signalpower ";
				}
				if (backpower - signalpower < signalThreshold)
				{
					isfit = false;
					exit_msg += "backpower - signalpower < " + signalThreshold;
				}
			}
			else
			{
				isfit = false;
				exit_msg += "lowerbound and upperbound out of range ";
			}
			
			return isfit;
		} // IsFit()

        /// <summary>
        /// sub-sample, assuming current sampleRate = 1.0;
        /// sub-sample at 1 / sampleRate
        /// </summary>
        /// <param name="sampleRate"></param>
        public void SubSample(int sampleRate)
        {
            _sampleRate = sampleRate;
            _fg = _fg.SubSample(_sampleRate);
            _length = _fg.Size;
            _ffit = new PZMath_vector(_length);
        } // SubSample()

		#region constructors

		/// <summary>
		/// empty structor
		/// </summary>
		public PZScanline() {}

		/// <summary>
		/// init constructor
		/// </summary>
		/// <param name="width">width of scanline</param>
		/// <param name="s">sample rate</param>
		/// <param name="inputc">init c</param>
		/// <param name="inputr">init r</param>
		/// <param name="inputa">init a</param>
		/// <param name="inputB">init B</param>
		public PZScanline(int width, int s, double inputc, double inputr, double inputmu, double inputB)
		{
			_widthOfScanline = width;
			_sampleRate = s;
			_length = _widthOfScanline * (int) _sampleRate;

            _fg = new PZMath_vector(_length);
			_fp = new PZPoint[_length];
			for (int i = 0; i < _length; i ++)
				_fp[i] = new PZPoint();
            _ffit = new PZMath_vector(_length);
			
			_c = inputc;
			_r = inputr;
			_mu = inputmu;
			_B = inputB;
		} // PZScanline(int, double, double, double, double, double)

        /// <summary>
        /// generate from 2 points
        /// </summary>
        /// <param name="start"></param>
        /// <param name="end"></param>
        public PZScanline(PZPoint start, PZPoint end)
        {
            // init members
            double width = start.Distance(end);
            _widthOfScanline = (int)width;
            _c = width / 2.0;
            _r = width / 2.0 - 2;
            _mu = 0.3;
            _B = 30;
            _sampleRate = 1;
            _length = _widthOfScanline * _sampleRate;
            _fp = new PZPoint[_length];
        }
        #endregion

        #region data access methods
        #endregion

        #region multifit methods
        /// <summary>
        /// do multi parameter fitting
        /// </summary>
        public void Fitting()
		{
			PZMath_multifit_fdfsolver_type T = new PZMath_multifit_fdfsolver_type();
			PZMath_multifit_fdfsolver s = new PZMath_multifit_fdfsolver();

			int i;
			int N = _length;	// number of samples
			int status;
			int iter = 0;
			
			// prepare StateParam object
			int n = N;
			int p = 12;	// number of model parameter {c, r, a, B}
						// and {x1, x2, x3, x4, lamda1, lamda2, lamda3, lamda4}

			PZMath_vector y = new PZMath_vector(_fg);	// vector of sample values
			double h = 1e-2; // step-size
			StateParam d = new StateParam(n, p, y, h, _sampleRate);

			// prepare model param
			double[] x_init = new double [p];	// initial parameter vaules
			x_init[0] = _c;
			x_init[1] = _r;
			x_init[2] = _mu;
			x_init[3] = _B;
			// constraints
			for (i = 4; i < 12; i ++)
				x_init[i] = 1.0;

			PZMath_vector x = new PZMath_vector(x_init);

			// prepare multifit solver
			PZMath_multifit_function_fdf f = new PZMath_multifit_function_fdf();
			PZMath_matrix covar = new PZMath_matrix(p, p);

			// with constraints
			f.f = new multifit_delegate_f(PZScanline.fit_f_with_constraints);
			f.df = new multifit_delegate_df(PZScanline.fit_df_with_constraints);
			f.fdf = new multifit_delegate_fdf(PZScanline.fit_fdf_with_constraints);
			f.n = n;
			f.p = p;
			f.parms = d;
			T.Init("PZMath_multifit_fdfsolver_lmsder");
			s.Alloc(T, n, p);
			s.Init(f, x);

			s.PrintState(iter);

			// do multi parameter fitting
			do
			{
				iter ++;
				status = s.Iterate();
				System.Diagnostics.Debug.WriteLine("status = " + status);
				s.PrintState(iter);

				if (status > 0)
					break;
				
				status = s.TestDelta (s.dx, s.x, 1e-4, 1e-4);
			}
			while (status == PZMath_errno.PZMath_CONTINUE && iter < 500);

			// calculate covariance matrix
			s.Covar(0.0, covar);
			covar.DebugWriteLine();

			System.Console.WriteLine("c = " + s.FIT(0) + " +/- " + s.ERR(0, covar));
			System.Console.WriteLine("r = " + s.FIT(1) + " +/- " + s.ERR(1, covar));
			System.Console.WriteLine("mu = " + s.FIT(2) + " +/- " + s.ERR(2, covar));
			System.Console.WriteLine("B = " + s.FIT(3) + " +/- " + s.ERR(3, covar));

			double chi = s.Chi();
			System.Console.WriteLine("chisq/dof = " + (System.Math.Pow (chi, 2.0) / (double) (n - p)));
			System.Console.WriteLine("state = " + status);

			// save estimated parameters
			_c = s.FIT(0);
			_r = s.FIT(1);
			_mu = s.FIT(2);
			_B = s.FIT(3);

			// save fitted profile
			CalculateFit();
		} // Fitting()

		/// <summary>
		/// calculate ffit values, in terms of the parameter estimation
		/// </summary>
		public void CalculateFit()
		{
			NewModelParam integration_param = new NewModelParam(_r, _c, _mu, _B, 0.0);

			// calculate the convolution, by PZMath_integration package
			PZMath_integration_workspace w = new PZMath_integration_workspace();
			w.Alloc(1000);
			PZMath_f F = new PZMath_f();
			F.f = new PZMath_delegate_f(f_integration);
			F.Parms = integration_param;
			double result = 0, error = 0;

			// calculate the target, 
			PZMath_vector f = new PZMath_vector(_widthOfScanline);

			for (int i = 0; i < _widthOfScanline; i ++)
			{
				double t = (double) i / (double) _sampleRate;
				double d = System.Math.Abs(t - _c);
				integration_param.x = t;
				PZMath_integration.Qags (F, -4, t + 4.0, 0, 1e-7, 1000, w, ref result, ref error);
				_ffit[i] = result;
			}
		} // CalculateFit()
		#endregion

		#region model functions for fitting
		/// <summary>
		/// simple model function
		/// </summary>
		/// <param name="x">model parameters</param>
		/// <param name="param">multifit state parameters</param>
		/// <param name="f">function values</param>
		/// <returns>PZMath_errno const</returns>
		public static int fit_f (PZMath_vector x, object param, PZMath_vector f)
		{
			// parse state data
			int n = (param as StateParam).n;
			PZMath_vector y = (param as StateParam).y;
			int samplerate = (param as StateParam).samplerate;

			// parse model parameters
			double c = x[0];
			double r = x[1];
			double a = x[2];
			double B = x[3];

			// calculate the f(x)
			for (int i = 0; i < n; i ++)
				// for each sample point
			{	
				double t = (double) i / (double) samplerate;
				double d = System.Math.Abs(t - c);
				if (d < r)
				{
					double result = System.Math.Sqrt(1 - d * d / r / r) * a + B;
					f[i] = result - y[i];
				}
				else
				{
					f[i] = B - y[i];
				}
			}
			return PZMath_errno.PZMath_SUCCESS;
		} // fit_f()

		/// <summary>
		/// function's derivatives
		/// </summary>
		/// <param name="x">model parameters</param>
		/// <param name="param">multifit state parameter</param>
		/// <param name="J">Jacobian matrix</param>
		/// <returns>PZMath_errno const</returns>
		public static int fit_df (PZMath_vector x, object param, PZMath_matrix J)
		{
			// parse state data
			int n = (param as StateParam).n;
			PZMath_vector y = (param as StateParam).y;
			int samplerate = (param as StateParam).samplerate;

			// parse model parameters
			double c = x[0];
			double r = x[1];
			double a = x[2];
			double B = x[3];
	
			// calculate Jacobian matrix
			for (int i = 0; i < n; i++)
				// for each sample point
			{
				// Jacobian matrix J(i, j) = dfi / dxj
				double t = (double) i / (double) samplerate;	// sample position
				double d = System.Math.Abs(t - c);
				if (d < r)
				{
					double s = System.Math.Sqrt(1 - d * d / r / r);
					J[i, 0] = a * (t - c) / r / r / s; // df/dc
					J[i, 1] = a * (t - c) * (t - c) / r / r / r / s ; // df/dr
					J[i, 2] = s; // df/da
					J[i, 3] = 1; // df/dB
				}
				else
				{
					J[i, 0] = 0; // df/dc
					J[i, 1] = 0 ; // df/dr
					J[i, 2] = 0; // df/da
					J[i, 3] = 1; // df/dB
				}	
			}

			return PZMath_errno.PZMath_SUCCESS;
		} // fit_df()

		/// <summary>
		/// model f/df function
		/// </summary>
		/// <param name="x">model parameters</param>
		/// <param name="param">state parameters</param>
		/// <param name="f">return function values</param>
		/// <param name="J">return Jacobian matrix</param>
		/// <returns></returns>
		public static int fit_fdf (PZMath_vector x, object param, PZMath_vector f, PZMath_matrix J)
		{
			fit_f(x, param, f);
			fit_df(x, param, J);
			return PZMath_errno.PZMath_SUCCESS; 
		} // fit_fdf()

		/// <summary>
		/// simple model function with constraints
		/// </summary>
		/// <param name="x">model parameters</param>
		/// <param name="param">multifit state parameters</param>
		/// <param name="f">function values</param>
		/// <returns>PZMath_errno const</returns>
		public static int fit_f_with_constraints (PZMath_vector x, object param, PZMath_vector f)
		{
			// parse state data
			int n = (param as StateParam).n;
			PZMath_vector y = (param as StateParam).y;
			int samplerate = (param as StateParam).samplerate;

			// parse model parameters
			double c = x[0];
			double r = x[1];
			double a = x[2];
			double B = x[3];

			double x1 = x[4];
			double x2 = x[5];
			double x3 = x[6];
			double x4 = x[7];

			double lamda1 = x[8];
			double lamda2 = x[9];
			double lamda3 = x[10];
			double lamda4 = x[11];

			// calculate the f(x)
			for (int i = 0; i < n; i ++)
				// for each sample point
			{	
				double t = (double) i / (double) samplerate;
				double d = System.Math.Abs(t - c);
				if (d < r)
				{
					double result = B + System.Math.Sqrt(1 - d * d / r / r) * a
						+ lamda1 * (x1 * x1 - r)
						+ lamda2 * (x2 * x2 - c)
						+ lamda3 * (x3 * x3 - a)
						+ lamda4 * (x4 * x4 - B);
					f[i] = result - y[i];
				}
				else
				{
					double result = B + lamda4 * (x4 * x4 - B);
					f[i] = result - y[i];
				}
			}
			return PZMath_errno.PZMath_SUCCESS;
		} // fit_f_with_constraints()

		/// <summary>
		/// function's derivatives with constraints
		/// </summary>
		/// <param name="x">model parameters</param>
		/// <param name="param">multifit state parameter</param>
		/// <param name="J">Jacobian matrix</param>
		/// <returns>PZMath_errno const</returns>
		public static int fit_df_with_constraints (PZMath_vector x, object param, PZMath_matrix J)
		{
			// parse state data
			int n = (param as StateParam).n;
			PZMath_vector y = (param as StateParam).y;
			int samplerate = (param as StateParam).samplerate;

			// parse model parameters
			double c = x[0];
			double r = x[1];
			double a = x[2];
			double B = x[3];

			double x1 = x[4];
			double x2 = x[5];
			double x3 = x[6];
			double x4 = x[7];

			double lamda1 = x[8];
			double lamda2 = x[9];
			double lamda3 = x[10];
			double lamda4 = x[11];
	
			// calculate Jacobian matrix
			for (int i = 0; i < n; i++)
				// for each sample point
			{
				// Jacobian matrix J(i, j) = dfi / dxj
				double t = (double) i / (double) samplerate;	// sample position
				double d = System.Math.Abs(t - c);
				if (d < r)
				{
					double s = System.Math.Sqrt(1 - d * d / r / r);
					J[i, 0] = a * (t - c) / r / r / s; // df/dc
					J[i, 1] = a * (t - c) * (t - c) / r / r / r / s ; // df/dr
					J[i, 2] = s; // df/da
					J[i, 3] = 1 - lamda4; // df/dB
					
					J[i, 4] = 2 * lamda1 * x1;	// df/dx1
					J[i, 5] = 2 * lamda2 * x2;	// df/dx2
					J[i, 6] = 2 * lamda3 * x3;	// df/dx3
					J[i, 7] = 2 * lamda4 * x4;	// df/dx4

					J[i, 8] = x1 * x1 - r;	// df/dlamda1
					J[i, 9] = x2 * x2 - c;	// df/dlamda2
					J[i, 10] = x3 * x3 - a; // df/dlamda3
					J[i, 11] = x4 * x4 - B; // df/dlamda4
				}
				else
				{
					J[i, 0] = 0; // df/dc
					J[i, 1] = 0 ; // df/dr
					J[i, 2] = 0; // df/da
					J[i, 3] = 1 - lamda4; // df/dB

					J[i, 4] = 0;	// df/dx1
					J[i, 5] = 0;	// df/dx2
					J[i, 6] = 0;	// df/dx3
					J[i, 7] = 2 * lamda4 * x4;	// df/dx4

					J[i, 8] = 0;	// df/dlamda1
					J[i, 9] = 0;	// df/dlamda2
					J[i, 10] = 0; // df/dlamda3
					J[i, 11] = x4 * x4 - B; // df/dlamda4
				}	
			}

			return PZMath_errno.PZMath_SUCCESS;
		} // fit_df_with_constraints()

		/// <summary>
		/// model f/df function with constraints
		/// </summary>
		/// <param name="x">model parameters</param>
		/// <param name="param">state parameters</param>
		/// <param name="f">return function values</param>
		/// <param name="J">return Jacobian matrix</param>
		/// <returns></returns>
		public static int fit_fdf_with_constraints (PZMath_vector x, object param, PZMath_vector f, PZMath_matrix J)
		{
			fit_f_with_constraints(x, param, f);
			fit_df_with_constraints(x, param, J);
			return PZMath_errno.PZMath_SUCCESS; 
		} // fit_fdf_with_constraints()
		#endregion

		#region util methods
		/// <summary>
		/// estimate direction from center point array
		/// by fitting a qudratic curve
		/// </summary>
		/// <param name="cp">center point array</param>
		/// <returns></returns>
		public static PZDirection EstimateDirection(PZPoint[] cp)
		{
			int i, j, n, m;
			n = cp.Length;		// number of consequtive points
			m = 3;				// qudratic curve

			// -- prepare model parameters
			double xi, yi, chisq;
			PZMath_matrix X, cov;
			PZMath_vector y, w, c;
			X = new PZMath_matrix(n, m);
			y = new PZMath_vector(n);
			w = new PZMath_vector(n);
			c = new PZMath_vector(m);
			cov = new PZMath_matrix(m, m);

			// -- data to be fitted
			for (i = 0; i < n; i ++)
			{
				xi = cp[i].y;	// xi = fy; yi = fx -- change the reference frame
				yi = cp[i].x;
				//System.Console.WriteLine(yi + " " + xi);
				for (j = 0; j < m; j ++)
                    X[i, j] = System.Math.Pow(xi, j);
				y[i] = yi;
				w[i] = 1.0;
			}

			// do linear fit
			PZMath_multifit_linear l = new PZMath_multifit_linear();
			l.Alloc(n, m);
			l.Wlinear(X, w, y, c, cov,out chisq);

			// return the derivative value at the center point

			PZDirection d = new PZDirection();
			double centery = cp[(int) (n / 2)].y;
			double slope = 0.0;
			for (j = 1; j < m; j ++)
                slope += (double)j * c[j] * System.Math.Pow(centery, j - 1); 
            //d.x = System.Math.Sin(System.Math.Atan(slope));
            //d.y = System.Math.Cos(System.Math.Atan(slope));
            double angle = System.Math.Atan(slope);
            double angleDegree = angle * 180 / System.Math.PI;
            d.x = System.Math.Sin(angle);
            d.y = System.Math.Cos(angle);
            

            // remove pi ambiguity
            double xIncrease = 0.0;
            double yIncrease = 0.0;
            for (i = 1; i < n; i++)
            {
                xIncrease += cp[i].x - cp[i - 1].x;
                yIncrease += cp[i].y - cp[i - 1].y;
            }
            // choose main increase direction
            if (System.Math.Abs(xIncrease) > System.Math.Abs(yIncrease))
            {
                if (d.x * xIncrease < 0)
                {
                    d.x *= -1.0;
                    d.y *= -1.0;
                }
            }
            else
            {
                if (d.y * yIncrease < 0)
                {
                    d.x *= -1.0;
                    d.y *= -1.0;
                }
            }
			return d;
		} // EstimateDirection()

        /// <summary>
        /// invert profile
        /// </summary>
        public void InvertProfile()
        {
            for (int i = 0; i < _widthOfScanline; i++)
            {
                _fg[i] = 255 - _fg[i];
            }
        } // InvertProfile()

		#endregion

		#region multimin methods
		public void NMMin()
			// use Nelder Mead Simplex to minimize the target
		{
			//int N = length;	// number of samples
            int N = _fg.Size;
			int status;
			int iter = 0;
			int totaliter = 1000;
			
			// prepare StateParam object
			int n = N;
			int p = 4;	// {x1, x2, x3, x4}
				// ---- old
						// number of model parameter {c, r, a, B}
						// and {x1, x2, x3, x4, lamda1, lamda2, lamda3, lamda4}

			PZMath_vector y = new PZMath_vector(_fg);	// vector of sample values
			double h = 1e-2; // step-size
			StateParam d = new StateParam(n, p, y, h, _sampleRate);

			// prepare model param
			double[] x_init = new double [p];	// initial parameter vaules
            x_init[0] = System.Math.Sqrt(_c);
            x_init[1] = System.Math.Sqrt(_r);
            x_init[2] = System.Math.Sqrt(_mu);
            x_init[3] = System.Math.Sqrt(_B);

			PZMath_vector x = new PZMath_vector(x_init);
			
			PZMath_multimin_fminimizer_type T = new PZMath_multimin_fminimizer_type();
			T.Init("PZMath_multimin_fminimizer_nmsimplex");

			PZMath_multimin_fminimizer s = new PZMath_multimin_fminimizer();

			/* Initial vertex size vector */
			PZMath_vector ss = new PZMath_vector(p);

			PZMath_multimin_function minex_func = new PZMath_multimin_function();

			double size;
			/* Set all step sizes to 0.5*/
			ss.SetAll(0.5);

			/* Initialize method and iterate */
			
			// with constraints
			//minex_func.f = new multimin_function(min_f);
			minex_func.f = new multimin_function(min_f_new);

			minex_func.n = p;
			minex_func.parms = d;
			s.Alloc(T, p);
			s.Init(minex_func, x, ss);

			do
			{
				iter++;
				status = s.Iterate();
      
				if (status > 0) 
					break;

				size = s.Size();
				status = s.TestSize (size, 1e-2);
				
				if (status == 0)
				{
					//System.Console.WriteLine  ("converged to minimum at" + iter);

					//System.Console.Write (iter + " ");

					//for (i = 0; i < p; i++)
					//{
					//	System.Console.Write (s.x[i] + " ");
					//}
					//System.Console.WriteLine ("f() = " + s.fval + "  size = " + size);
				}
			}
			while (status == -2 && iter < totaliter);
			if (iter == totaliter)
				System.Console.Write("disconvergent... ");
			// save estimated parameters
			_c = s.x[0] * s.x[0];
			_r = s.x[1] * s.x[1];
			_mu = s.x[2] * s.x[2];
			_B = s.x[3] * s.x[3];

			// save fitted profile
			CalculateMin();
            //c++;
		} // NMMin()
		
		public void CalculateMin()
		{
            // disregarding convolution with PSF
            double theta, t, d, l;
            double I0 = 255.0;
            for (int i = 0; i < _length; i++)
            {
                t = (double)i / (double)_sampleRate;
                d = System.Math.Abs(t - _c);  // d = |x - c|
                // |x - c| < r;
                if (d < _r)
                {
                    theta = System.Math.Acos(d / _r);
                    l = 2 * _r * System.Math.Sin(theta);
                }
                else
                    l = 0.0;
                _ffit[i] = I0 * System.Math.Exp(-_mu * l) - _B;
            }

            
            //NewModelParam integration_param = new NewModelParam(_c, _r, _mu, _B, 0.0);

            //// calculate the convolution, by PZMath_integration package
            //PZMath_integration_workspace w = new PZMath_integration_workspace();
            //w.Alloc(1000);
            //PZMath_f F = new PZMath_f();
            //F.f = new PZMath_delegate_f(f_integration);
            //F.Parms = integration_param;
            //double result = 0, error = 0;

            //PZMath_vector f = new PZMath_vector(_widthOfScanline);

            //for (int i = 0; i < _widthOfScanline; i ++)
            //{
            //    double t = (double) i / (double) _sampleRate;
            //    double d = System.Math.Abs(t - _c);
            //    integration_param.x = t;
            //    PZMath_integration.Qags (F, -3.5, t + 3.5, 0.001, 1e-7, 1000, w, ref result, ref error);
            //    _ffit[i] = result;
            //}

		} // CalculateMin()
		#endregion

		#region model function for min
		public static double min_f (PZMath_vector x, object param)
		{
			// parse state data
			int n = (param as StateParam).n;
			PZMath_vector y = (param as StateParam).y;
			int samplerate = (param as StateParam).samplerate;

			// parse model parameters
			double c = x[0];
			double r = x[1];
			double a = x[2];
			double B = x[3];

			// calculate the target, 
			double target = 0.0;
			PZMath_vector f = new PZMath_vector(n);

			for (int i = 0; i < n; i ++)
				// for each sample point
			{	
				double t = (double) i / (double) samplerate;
				double d = System.Math.Abs(t - c);
				if (d < r)
				{
					double result = System.Math.Sqrt(1 - d * d / r / r) * a + B;
					f[i] = result - y[i];
				}
				else
				{
					f[i] = B - y[i];
				}
				target += f[i] * f[i];
			}
            return System.Math.Sqrt(target);
		} // min_f()

		public static double min_f_with_constraints (PZMath_vector x, object param)
		{
			// parse state data
			int n = (param as StateParam).n;
			PZMath_vector y = (param as StateParam).y;
			int samplerate = (param as StateParam).samplerate;

			// parse model parameters
			double c = x[0];
			double r = x[1];
			double a = x[2];
			double B = x[3];
			
			double x1 = x[4];
			double x2 = x[5];
			double x3 = x[6];
			double x4 = x[7];

			double lamda1 = x[8];
			double lamda2 = x[9];
			double lamda3 = x[10];
			double lamda4 = x[11];

			// calculate the target, 
			double target = 0.0;
			PZMath_vector f = new PZMath_vector(n);

			for (int i = 0; i < n; i ++)
				// for each sample point
			{	
				double t = (double) i / (double) samplerate;
				double d = System.Math.Abs(t - c);
				if (d < r)
				{
					double result = B + System.Math.Sqrt(1 - d * d / r / r) * a
						+ lamda1 * (x1 * x1 - r)
						+ lamda2 * (x2 * x2 - c)
						+ lamda3 * (x3 * x3 - a)
						+ lamda4 * (x4 * x4 - B);
					f[i] = result - y[i];
				}
				else
				{
					double result = B + lamda4 * (x4 * x4 - B);
					f[i] = result - y[i];
				}
				target += f[i] * f[i];
			}
            return System.Math.Sqrt(target);
		} // min_f_with_constraints()

		public static double min_f_new (PZMath_vector x, object param)
			// integration model with constraint
		{
			// parse state data
			int n = (param as StateParam).n;
			PZMath_vector y = (param as StateParam).y;
			int samplerate = (param as StateParam).samplerate;
			
			// parse model parameters
			double x1 = x[0];
			double x2 = x[1];
			double x3 = x[2];
			double x4 = x[3];
			
			double c = x1 * x1;
			double r = x2 * x2;
			double mu = x3 * x3;
			double B = x4 * x4;
			
            // disregarding convolution with PSF
            double target = 0.0;
            double theta, t, d, l, f;
            double I0 = 255.0;
            for (int i = 0; i < n; i++)
            {
                t = (double)i / (double)samplerate;
                d = System.Math.Abs(t - c);  // d = |x - c|
                // |x - c| < r;
                if (d < r)
                {
                    theta = System.Math.Acos(d / r);
                    l = 2 * r * System.Math.Sin(theta);                    
                }
                else
                    l = 0.0;
                f = I0 * System.Math.Exp(-mu * l) - B;
                f -= y[i];
                target += f * f;
            }

            //// (c, r, mu, B, x)
            //NewModelParam integration_param = new NewModelParam(c, r, mu, B, 0.0);

            //// calculate the convolution, by PZMath_integration package
            //PZMath_integration_workspace w = new PZMath_integration_workspace();
            //w.Alloc(1000);
            //PZMath_f F = new PZMath_f();
            //F.f = new PZMath_delegate_f(f_integration);
            //F.Parms = integration_param;
            //double result = 0, error = 0;

            //// calculate the target, 
            //double target = 0.0;
            //PZMath_vector f = new PZMath_vector(n);

            //for (int i = 0; i < n; i ++)
            //{
            //    double t = (double) i / (double) samplerate;
            //    double d = System.Math.Abs(t - c);
            //    integration_param.x = t;
            //    PZMath_integration.Qags (F, -3.5, t + 3.5, 0.001, 1e-7, 1000, w, ref result, ref error);
            //    f[i] = result - y[i];
            //    /*
            //    if (d < r)
            //    {
            //        result += (lamda1 * (x1 * x1 - r)
            //            + lamda2 * (x2 * x2 - c)
            //            + lamda3 * (x3 * x3 - mu)
            //            + lamda4 * (x4 * x4 - B));
            //        f[i] = result - y[i];
            //    }
            //    else
            //    {
            //        result += lamda4 * (x4 * x4 - B);
            //        f[i] = result - y[i];
            //    }
            //    */
            //    target += f[i] * f[i];
            //}


            return System.Math.Sqrt(target);
		} // min_f_new()
		
		/// <summary>
		/// integration f
		/// </summary>
		/// <param name="x"></param>
		/// <param name="param"></param>
		/// <returns></returns>
		public static double f_integration (double t, object param)
		{
			// parse parameters
			double r = (param as NewModelParam).r;
			double c = (param as NewModelParam).c;
			double mu = (param as NewModelParam).mu;
			double B = (param as NewModelParam).B;
			double x = (param as NewModelParam).x;

			// projection modle
			// a = r;
			// l = 2.0 * sqrt(pow(a, 2) - pow((t - c), 2));
			// attenuated = 255 *  exp(-1 * mu * l) - background;
			double a = r;
			double lowerbound = c - a;
			double upperbound = c + a;
			double l;
			if ((t > lowerbound) && (t < upperbound))
                l = 2.0 * System.Math.Sqrt(System.Math.Pow(a, 2) - System.Math.Pow((t - c), 2));
			else
				l = 0.0;
            double attenuated = 255 * System.Math.Exp(-1 * mu * l) - B;

			// PSF - thegma = 1
			// g = 1.0 / sqrt(2.0 * pi) * exp(-1.0 * pow(t, 2) / 2.0);
			// g(x - t):= t -> (x - t)System.
            double g = 1.0 / System.Math.Sqrt(2.0 * System.Math.PI) * System.Math.Exp(-1.0 * System.Math.Pow(x - t, 2) / 2.0);
	
			// convolution function
			double f_integration = attenuated * g;

			return f_integration;
		} // f_integration()
		#endregion

		#region file IO methods
        /// <summary>
        /// write scanline profile and fitting result to txt file
        /// </summary>
        /// <param name="filename"></param>
		public void WriteFile(string filename)
		{
			FileStream file = new FileStream(filename, FileMode.Create, FileAccess.Write);
			StreamWriter sw = new StreamWriter(file);
			for (int i = 0; i < _length; i ++)
				sw.WriteLine(_fg[i].ToString() + " " + _ffit[i].ToString());
			sw.Close();
			file.Close();
		} // WriteFile()

        public void ReadFile(string fileName)
        {
            // TODO PZScanline::ReadFile(string)
        } // ReadFile()

        /// <summary>
        /// draw scanline on source image
        /// </summary>
        /// <param name="fileName"></param>
        /// <param name="image"></param>
        public void DrawScanline(string fileName, Bitmap image)
        {
            Bitmap srcImage = new Bitmap(image);
            eee.Sheffield.PZ.Imaging.Image.StandardizeImageTo24bppRGB(ref srcImage);

            int width = image.Width;
            int height = image.Height;
            for (int i = 0; i < _length; i++)
            {
                int x = (int)_fp[i].x;
                int y = (int)_fp[i].y;
                if (x >= 0 && x < width && y >= 0 && y < height)
                    srcImage.SetPixel(x, y, Color.Red);
            }
            srcImage.Save(fileName, ImageFormat.Bmp);

        } // DrawScanline()
        #endregion
    } // PZScanline

    /// <summary>
    /// model parameters
    /// </summary>
    public class ModelParam
	{
		public double c;	// center point	
		public double r;	// radius
		public double a;	// peak intensity of vessel
		public double B;	// background intensity
		
		public ModelParam (double inputc, double inputr, double inputa, double inputB)
			// init constructor
		{
			c = inputc;
			r = inputr;
			a = inputa;
			B = inputB;
		} // ModelParam(double, double, double, double)
	} // ModelParam

	/// <summary>
	/// new model parameters
	/// </summary>
	public class NewModelParam
	{
		public double c;	// center point	
		public double r;	// radius
		public double mu;	// 
		public double B;	// background intensity
		public double x;	
		
		public NewModelParam (double inputc, double inputr, double inputmu, double inputB, double inputX)
			// init constructor
		{
			c = inputc;
			r = inputr;
			mu = inputmu;
			B = inputB;
			x = inputX;
		} // NewModelParam()
	} // NewModelParam

	/// <summary>
	/// multifit state parameters
	/// </summary>
	public class StateParam
	{
		public int n;		// number of observed values
		public int p;		// number of parameters
		public PZMath_vector y;	// observed values
		public double h;			// step-size
		public int samplerate;	// sample rate

		/// <summary>
		/// init constructor
		/// </summary>
		/// <param name="inputn">number of samples</param>
		/// <param name="inputp">number of model parameters</param>
		/// <param name="inputy">sample values</param>
		/// <param name="inputh">step (useless now)</param>
		/// <param name="inputsamplerate">sample rate</param>
		public StateParam(int inputn, int inputp, PZMath_vector inputy, double inputh, int inputsamplerate)
		{
			n = inputn;
			p = inputp;
			y = inputy;
			h = inputh;
			samplerate = inputsamplerate;
		} // StateParam(int, int, PZMath_vector, double, int)
	} // StateParam 

	/// <summary>
	/// model parameters with constraints
	/// </summary>
	public class ModelParamWithConstraints
	{
		public double c;	// center point	
		public double r;	// radius
		public double a;	// peak intensity of vessel
		public double B;	// background intensity
		
		// loose varialbes
		public double x1;	
		public double x2;
		public double x3;
		public double x4;
		
		// lagrange factors
		public double lamda1;
		public double lamda2;
		public double lamda3;
		public double lamda4;

		public ModelParamWithConstraints (double inputc, double inputr, double inputa, double inputB)
			// init constructor
		{
			c = inputc;
			r = inputr;
			a = inputa;
			B = inputB;

			x1 = 1.0;
			x2 = 1.0;
			x3 = 1.0;
			x4 = 1.0;

			lamda1 = 1.0;
			lamda2 = 1.0;
			lamda3 = 1.0;
			lamda4 = 1.0;
		} // ModelParam(double, double, double, double)
	} // ModelParamWithConstraints

}
